#include "read_data.h"

double ns_to_seconds(uint64_t nanoseconds) {
    const uint64_t ns_per_second = 1000000000ULL;
    uint64_t whole_seconds = nanoseconds / ns_per_second;
    uint64_t fraction_ns = nanoseconds % ns_per_second;
    return whole_seconds + static_cast<double>(fraction_ns) / ns_per_second;
}

read_data::read_data(const std::string& path, const SensorDataType& sensor_data_type) : sensor_data_type_(sensor_data_type) {
    path_ = path;
    origin_timestamp = 0.0;
    if(sensor_data_type_ == SensorDataType::KAIST) {
        ext_rot_vehicle2laser_ << 0.00130201, 0.796097, 0.605167, 0.999999, -0.000419027, -0.00160026, -0.00102038, 0.605169, -0.796097;
        ext_trans_vehicle2laser_ << 0.8349,-0.0126869,1.76416;      
    }

    if(sensor_data_type_ == SensorDataType::NCLT) {
        ext_rot_vehicle2laser_  = eulerToRotationMatrix(180.0f,0.0f,0.0f);
        // float ext_roll = 180.0f * M_PI/180.0f;
        // ext_rot_vehicle2laser_  = eulerToRotationMatrix(ext_roll,0.0f,0.0f);
        ext_trans_vehicle2laser_ << 0.28, 0.0, -0.44;  
    }
    std::cout << "ext_rot_vehicle2laser_ : \n" << ext_rot_vehicle2laser_ << std::endl;
    std::cout << "ext_trans_vehicle2laser_ : \n " << ext_trans_vehicle2laser_ << std::endl;
}

std::vector<std::string> read_data::split(std::string str, std::string pattern) {
	std::string::size_type pos;
	std::vector<std::string> result;
	str += pattern;
	int size = str.size();

	for (int i = 0; i < size; i++) {
		pos = str.find(pattern, i);

		if (pos < size) {
			std::string s = str.substr(i, pos - i);
			result.push_back(s);
			i = pos + pattern.size() - 1;
		}
	}
	return result;
}

bool read_data::LoadSensorData(const std::string& encoder_file_path, std::unordered_map<std::string, std::string>* time_data_map) {
    std::ifstream encoder_file(encoder_file_path);
    if (!encoder_file.is_open()) {
        std::cout << "[LoadSensorData]: Failed to open encoder file." << std::endl;
        return false;
    } 

    std::string line_str, time_str;
    while (std::getline(encoder_file, line_str)) {
        std::stringstream ss(line_str);
        if (!std::getline(ss, time_str, ',')) {
            std::cout << "[LoadSensorData]: Find a bad line in the encoder file.: " << line_str << std::endl;
            return false;
        }
        
        // std::cout << "[main]: time_str : " << time_str << std::endl;
        // std::cout << "[main]: line_str : " << line_str << std::endl;

        time_data_map->emplace(time_str, line_str);
    }

    return true;
}

bool read_data::read_only_kaist_data(std::unordered_map<std::string, std::string>& time_encoder_map, 
                              		 std::unordered_map<std::string, std::string>& time_imu_map,
                              		 std::unordered_map<std::string, std::string>& time_gps_map, 
                              		 std::unordered_map<std::string, std::string>& time_vrs_gps_map,
                                     std::unordered_map<std::string, std::string>& time_sick_middle_map,
                                     std::unordered_map<std::string, std::string>& time_velodyne_left_map) {
	bool state = true;
	// Load all encoder data to buffer.
    if (!LoadSensorData(path_ + "sensor_data/encoder.csv", &time_encoder_map)) {
        std::cout << "[main]: Failed to load encoder data." << std::endl;
        state = false;
    } 

    // Load all imu date to buffer.
    if (!LoadSensorData(path_ + "sensor_data/xsens_imu.csv", &time_imu_map)) {
        std::cout << "[main]: Failed to load imu data." << std::endl;
        state = false;
    } 

    // Load all gps data to buffer.
    if (!LoadSensorData(path_ + "sensor_data/gps.csv", &time_gps_map)) {
        std::cout << "[main]: Failed to load gps data." << std::endl;
        state = false;
    } 

    if (!LoadSensorData(path_ + "sensor_data/vrs_gps.csv", &time_vrs_gps_map)) {
        std::cout << "[main]: Failed to load gps data." << std::endl;
        state = false;
    } 	

    if (!LoadSensorData(path_ + "sensor_data/SICK_middle_stamp.csv", &time_sick_middle_map)) {
        std::cout << "[main]: Failed to load SICK_middle data." << std::endl;
        state = false;
    }  

    if (!LoadSensorData(path_ + "sensor_data/VLP_left_stamp.csv", &time_velodyne_left_map)) {
        std::cout << "[main]: Failed to load VLP_left_stamp data." << std::endl;
        state = false;
    }  
    
    std::ifstream file_data_stamp(path_ + "sensor_data/data_stamp.csv");
    if (!file_data_stamp.is_open()) {
        std::cout << "[main]: Failed to open data_stamp data." << std::endl;
    }

    std::vector<std::string> line_data_vec;
    line_data_vec.reserve(3);
    std::string line_str, value_str;

	std::vector<double> times;

    while (std::getline(file_data_stamp, line_str)) {
        line_data_vec.clear();
        std::stringstream ss(line_str);
        while (std::getline(ss, value_str, ',')) { line_data_vec.push_back(value_str); }

        const double kToSecond = 1e-9;
        const std::string& time_str = line_data_vec[0];

        const double timestamp = std::stod(time_str) * kToSecond;
    	times.push_back(timestamp);
    	if(times.size() > 200) {
    		break;
    	}
    }

    std::sort(times.begin(),times.end());
    
    origin_timestamp = times[0] - 10.0;

    return state;
}

std::unordered_map<std::string, std::string> read_data::read_only_nclt_data(const std::string& file_path) {
    bool state = true;
    std::unordered_map<std::string, std::string> time_and_sensor_type_map;
    // Load all encoder data to buffer.
    if (!LoadSensorData(path_ + file_path, &time_and_sensor_type_map)) {
        std::cout << "[main]: Failed to load encoder data." << std::endl;
        state = false;
    } 

    return time_and_sensor_type_map;
}

std::unordered_map<std::string, LaserScan> read_data::read_only_nclt_laser_data(const std::string& file_path) {
    
    std::unordered_map<std::string, LaserScan> time_and_sensor_data_map;

    constexpr int NUM_HITS = 1081;
    constexpr float RAD_START = -2.35619449f; // -135度
    constexpr float RAD_STEP = 0.004363323f;  // 0.25度
    bool has_intensity = false;

    std::ifstream file(path_ + file_path, std::ios::binary);
    if (!file) return time_and_sensor_data_map;

    // 预计算角度
    std::vector<float> angles(NUM_HITS);
    for (int i = 0; i < NUM_HITS; ++i) {
        angles[i] = RAD_START + i * RAD_STEP;
    }
    
    float scaling = 0.005f;
    float offset = -100.0f;
    bool is_first_frame = true;
    while (true) {

        LaserScan2d scan;

        LaserScan laser_scan;
        uint64_t timestamp;
        if (!file.read(reinterpret_cast<char*>(&timestamp), 8)) {
            break;
        }

        scan.timestamp = timestamp;

        // 读取原始距离数据
        std::vector<uint16_t> raw_data(NUM_HITS);
        if (!file.read(reinterpret_cast<char*>(raw_data.data()), NUM_HITS*2)) {
            break;
        }

        if (has_intensity) {
            scan.intensities.resize(NUM_HITS);
            if (!file.read(reinterpret_cast<char*>(scan.intensities.data()), NUM_HITS*2))
                break;
        }
        laser_scan.angle_min = RAD_START;
        laser_scan.angle_max = RAD_START + (NUM_HITS-1) * RAD_STEP;
        laser_scan.angle_increment = RAD_STEP;
        laser_scan.time_increment = 0;
        laser_scan.scan_time = ns_to_seconds(timestamp);
        laser_scan.range_min = 0.f;
        laser_scan.range_max = 30;

        // LOG(INFO) << std::setprecision(15) << "timestamp ======= : " << timestamp << std::endl;
        // LOG(INFO) << std::setprecision(16) << "laser_scan.scan_time ======= : " << laser_scan.scan_time << std::endl;

        // sleep(5);

        // 转换数据
        laser_scan.ranges.resize(NUM_HITS);
        
        if (has_intensity) {
            laser_scan.intensities.resize(NUM_HITS);
        }

        scan.distances.resize(NUM_HITS);
        scan.x_coords.resize(NUM_HITS);
        scan.y_coords.resize(NUM_HITS);
        
        for (int i = 0; i < NUM_HITS; ++i) {
            laser_scan.ranges[i] = raw_data[i] * scaling + offset;
            if (has_intensity) {
                laser_scan.intensities[i] = scan.intensities[i];
            }

            scan.distances[i] = raw_data[i] * scaling + offset;
            scan.x_coords[i] = scan.distances[i] * std::cos(angles[i]);
            scan.y_coords[i] = scan.distances[i] * std::sin(angles[i]);
        }

        laser_scan.x_coords = scan.x_coords;
        laser_scan.y_coords = scan.y_coords;

        // if(is_first_frame) {
        //     LOG(INFO) << "timestamp ======= : " << timestamp << std::endl;
        //     LOG(INFO) << "std::to_string(timestamp) ======= : " << std::to_string(timestamp).size() << std::endl;
        //     is_first_frame = true;
        // }

        time_and_sensor_data_map[std::to_string(timestamp)] = laser_scan;
    }

    return time_and_sensor_data_map;
}
